/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.CriteriaCollection;
import edu.wpi.first.wpilibj.image.NIVision.MeasurementType;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;
import edu.wpi.first.wpilibj.image.RGBImage;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.DriveWithJoy;

/**
 *
 * @author Team 3502
 */
public class DriveTrainPID extends PIDSubsystem {
    private static final double Kp = 0.1;
    private static final double Ki = 0.0;
    private static final double Kd = 0.0;

    //Put methods for controlling this subsystem here. Call these from Commands.
    private RobotDrive drive;
    private Gyro gyro;
    private ADXL345_I2C accel;
    private Encoder encoderLeft;
    private Encoder encoderRight;

    private double gyro_o, gyro_n, gyro_filt; //previous gyro readout, new gyro readout, filtered gyro;
    private double acc_x, acc_y, acc_z; //accelerometer readouts (x, y, z axes)
    private double accel_o, accel_n, accel_filt;
    private double tilt, tilt_filt; //a.k.a. angle of inclination on bridge
    //double ky; //accelerometer scaling factor for calculating tilt
    private double kx; //accelerometer scaling factor for calculating tilt\
    private double encLt_dist, encRt_dist;
    
    private double targetx, targety;
    private ColorImage image;
    private AxisCamera camera;          // the axis camera object (connected to the switch)
    private CriteriaCollection cc;      // the criteria for doing the particle filter operation

    // Initialize your subsystem here
    public DriveTrainPID() {
        super("DriveTrainPID", Kp, Ki, Kd);

        // Use these to get going:
        // setSetpoint() -  Sets where the PID controller should move the system to
        // enable() - Enables the PID controller.

        gyro = new Gyro(RobotMap.gyrochan);
        accel = new ADXL345_I2C (1, ADXL345_I2C.DataFormat_Range.k4G); //can use k2, k4, k8, or k16 -- ex. k8G sets range as +/- 8G
        encoderLeft = new Encoder(RobotMap.encoderLeftA, RobotMap.encoderLeftB);
        encoderRight = new Encoder(RobotMap.encoderRightA, RobotMap.encoderRightB);
        drive = new RobotDrive(RobotMap.frontLeftMotor, RobotMap.rearLeftMotor, RobotMap.frontRightMotor, RobotMap.rearRightMotor);
        drive.setSafetyEnabled(false);
        //ky = 90.0/56.25; //accelerometer y axis scaling factor:  at 90 deg (actual), accelerometer read about 57 degrees (raw)
        kx = 90.0/59.0; //accelerometer z axis scaling factor:

        encoderLeft.setDistancePerPulse(3.1416 * 8.0 / 360.0); //circumference div by 360 pulses per rev
        encoderRight.setDistancePerPulse(3.1416 * 8.0 / 360.0);        

        camera = AxisCamera.getInstance();  // get an instance ofthe camera
        cc = new CriteriaCollection();      // create the criteria for the particle filter

        //criteria for our test pictures
        cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 20, 325, false);
        cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 19, 260, false);

        //use different criteria for live shots which are a 480 x 640 image
        //cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 20, 600, false);
        //cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 19, 450, false);
    }
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        setDefaultCommand(new DriveWithJoy());
    }

    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:
        // yourPot.getAverageVoltage() / kYourMaxVoltage;
        targetx = checktarget();
        return targetx;
    }

    protected void usePIDOutput(double output) {
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);
        if (output < -0.1){
            System.out.println("Target is to the left");
            driveWithJoy(0, -0.375);
        }
        else if (output > 0.1){
            System.out.println("Target is to the right");
            driveWithJoy(0, 0.375);
        }
    }

    public void driveWithJoy(double y, double twist){
        drive.arcadeDrive(-y, -twist);
        //System.out.println("Drive Me");
    }

    //Process images to find the highest target
    public double findtarget(){
        try {
            image = camera.getImage(); //gets a snapshot from the camera
            //image = new RGBImage("/1a.JPG"); //use this with test images
            //image.write("/3502getImage.jpg"); //saves the snapshot image to a file

            //use with test images
            BinaryImage thresholdImage = image.thresholdRGB(0, 255, 0, 255, 150, 255);   // filter out all but green colors
            
            //use with jazmins car
            //BinaryImage thresholdImage = image.thresholdRGB(0, 132, 102, 253, 0, 78);   // filter out all but green colors

            BinaryImage bigObjectsImage = thresholdImage.removeSmallObjects(false, 1);  // remove small artifacts
            BinaryImage convexHullImage = bigObjectsImage.convexHull(false);          // fill in occluded rectangles
            BinaryImage filteredImage = convexHullImage.particleFilter(cc);           // find filled in rectangles

            System.out.println("# of Particles Found = " + filteredImage.getNumberParticles() + Timer.getFPGATimestamp());
            if (filteredImage.getNumberParticles() != 0) {
                ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();  //get list of results
                targety = 2; //set y target outside of image to start
                for (int i = 0; i < reports.length; i++) {                                // print results
                    ParticleAnalysisReport r = reports[i];
                    System.out.print("Particle " + i + ":  Center of mass x = " + r.center_mass_x + ",  ");
                    System.out.println("Center of mass y = " + r.center_mass_y);
                    //Find the highest hoop
                    if (r.center_mass_y_normalized < targety){
                        targetx = r.center_mass_x_normalized;
                        targety = r.center_mass_y_normalized;
                        System.out.println("Target Particle Number = " + i);
                    }
                }
                System.out.println("targetx = " + targetx);
            }
            else {
                System.out.println("NO TARGETS FOUND!!!");
            }

            /**
             * all images in Java must be freed after they are used since they are allocated out
             * of C data structures. Not calling free() will cause the memory to accumulate over
             * each pass of this loop.
             */
            filteredImage.free();
            convexHullImage.free();
            bigObjectsImage.free();
            thresholdImage.free();
            image.free();

        } catch (AxisCameraException ex) {        // this is needed if the camera.getImage() is called
            System.out.println("AxisCameraException");
            //ex.printStackTrace();
        } catch (NIVisionException ex) {
            System.out.println("NIVisionException");
            //ex.printStackTrace();
        }
        return targetx;
    }

    //checks the x position of the highest target found in findtarget()
    public double checktarget(){
        try {
            image = camera.getImage(); //gets a snapshot from the camera
            //image = new RGBImage("/1a.JPG"); //use this with test images
            //image.write("/3502getImage.jpg"); //saves the snapshot image to a file

            //use with test images
            //BinaryImage thresholdImage = image.thresholdRGB(0, 255, 0, 255, 150, 255);   // filter out all but green colors

            //use with jazmins car
            BinaryImage thresholdImage = image.thresholdRGB(0, 132, 102, 253, 0, 78);   // filter out all but green colors

            BinaryImage bigObjectsImage = thresholdImage.removeSmallObjects(false, 1);  // remove small artifacts
            BinaryImage convexHullImage = bigObjectsImage.convexHull(false);          // fill in occluded rectangles
            BinaryImage filteredImage = convexHullImage.particleFilter(cc);           // find filled in rectangles

            ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();  //get list of results

            for (int i = 0; i < reports.length; i++) {                                // print results
                ParticleAnalysisReport r = reports[i];
                System.out.print("Particle " + i + ":  Center of mass x = " + r.center_mass_x + ",  ");
                System.out.println("Center of mass y = " + r.center_mass_y);
                //Make sure that the target stays the same as the original one
                if (r.center_mass_y_normalized > (targety-0.1) && r.center_mass_y_normalized < (targety+0.1)){
                    targetx = r.center_mass_x_normalized;
                }
            }
            System.out.println("targetx = " + targetx);

            //3502 TESTING:
            //thresholdImage.write("/3502thresh.jpg");
            //bigObjectsImage.write("/3502big.jpg");
            //convexHullImage.write("/3502convex.jpg");  //
            //filteredImage.write("/3502filt.jpg");

            /**
             * all images in Java must be freed after they are used since they are allocated out
             * of C data structures. Not calling free() will cause the memory to accumulate over
             * each pass of this loop.
             */
            filteredImage.free();
            convexHullImage.free();
            bigObjectsImage.free();
            thresholdImage.free();
            image.free();

        } catch (AxisCameraException ex) {        // this is needed if the camera.getImage() is called
            System.out.println("AxisCameraException");
            //ex.printStackTrace();
        } catch (NIVisionException ex) {
            System.out.println("NIVisionException");
            //ex.printStackTrace();
        }
        return targetx;
    }

    public void initsensors(){
        gyro.reset();
        gyro_o = gyro.getAngle();
        //accel_o = accel.getAcceleration(ADXL345_I2C.Axes.kY)* ky * 180.0 / 3.1416;
        accel_o = accel.getAcceleration(ADXL345_I2C.Axes.kX)* kx * 180.0 / 3.1416;
        encoderLeft.reset();
        encoderLeft.start();
        encoderRight.reset();
        encoderRight.start();
    }
    
    public void readgyro(){
        gyro_n = gyro.getAngle();
        //System.out.println("Gyro Angle (Raw) = " + gyro_n);
        gyro_filt = (gyro_o - gyro_n) / 2; //high pass filter
        gyro_o = gyro_n;
    }

    public void readaccel(){
        acc_x = accel.getAcceleration(ADXL345_I2C.Axes.kX);
        //System.out.println("Acceleration (X axis)= " + accread);
        acc_y = accel.getAcceleration(ADXL345_I2C.Axes.kY);
        //System.out.println("Acceleration (Y axis)= " + accread);
        acc_z = accel.getAcceleration(ADXL345_I2C.Axes.kZ);
        //System.out.println("Acceleration (Z axis)= " + accread);
        //accel_n = acc_y * ky * 180.0 / 3.1416; //convert to degrees
        accel_n = acc_x * kx * 180.0 / 3.1416; //convert to degrees
        accel_filt = (accel_o + accel_n) / 2; //low pass filter
        accel_o = accel_n;
    }

    public void findtilt(){
        //calculate tilt a.k.a. angle of inclination
        //tilt = acc_y * ky * 180.0 / 3.1416;
        tilt_filt = (gyro_filt + accel_filt) - 4.25 + 90.0; //complimentary filter, adjusting for mounting angle, and shifting 90 degrees
    }

    public double readencoders(){
        encLt_dist = Math.abs(encoderLeft.getDistance());
        encRt_dist = Math.abs(encoderRight.getDistance());
        
        //Take the lesser of the two numbers in case one of the wheels slips
        if (encLt_dist < encRt_dist){
            return encLt_dist;
        }
        else if (encRt_dist < encLt_dist){
            return encRt_dist;
        }
        else {
            return encLt_dist;
        }
    }

    public void readsensors(){
        //SmartDashboard.putDouble("Gyro Angle (Raw/Drifting)", gyro_n);
        //SmartDashboard.putDouble("Gyro Angle (Filtered)", gyro_filt);
        //SmartDashboard.putDouble("Accelerometer X in degrees (Raw)", accel_n);
        //SmartDashboard.putDouble("Accelerometer X in degrees (Filtered)", accel_filt);
        //SmartDashboard.putDouble("Tilt Angle X in degrees (Filtered)", tilt_filt);
        //SmartDashboard.putDouble("Left Encoder Distance (inches)", encLt_dist);
        //SmartDashboard.putDouble("Right Encoder Distance (inches)", encRt_dist);
    }

}